

// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/gicp.h>

#include <geometry_utils/Transform3.h>
#include <sstream>
#include <fstream>
#include <iostream>
#include <string>
#include "time_counter.hpp"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <point_new.h>
#include <mutex>
using namespace std;

Eigen::Affine3f transform0;

ros::NodeHandle &get_handle()
{
    static ros::NodeHandle nh;
    return nh;
}
template <class T>
ros::Publisher &get_publisher(const std::string &topic)
{
    static std::map<std::string, ros::Publisher> publisher_map;
    if (publisher_map.find(topic) == publisher_map.end())
    {
        publisher_map[topic] = get_handle().advertise<T>(topic, 1000);
    }
    return publisher_map[topic];
}

template <typename Point>
void pubpc(std::string topic, pcl::PointCloud<Point> &pc, std::string frame = "base_link")
{
    sensor_msgs::PointCloud2 msgs;
    pcl::toROSMsg(pc, msgs);
    msgs.header.frame_id = frame;
    get_publisher<sensor_msgs::PointCloud2>(topic).publish(msgs);
}

std::mutex mtx;
pcl::PointCloud<pcl::PointXYZI> frames3;
pcl::PointCloud<pcl::PointXYZI> frames;
void velodyne_callback3(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    mtx.lock();
    pcl::PointCloud<pcl::PointXYZI> temp;
    pcl::fromROSMsg(*cloud, frames3);
    for(auto& pt:frames3){
        if(pt.x>6&&pt.x<12&&pt.y>8&&pt.y<14&&pt.z>-0.5&&pt.z<2){
            temp.push_back(pt);
        }
    }
    pcl::transformPointCloud(temp, frames3, transform0);
    mtx.unlock();
    pubpc("target",frames3);
}

void velodyne_callback(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    pcl::PointCloud<pcl::PointXYZI> temp;
    pcl::fromROSMsg(*cloud, frames);
    for(auto& pt:frames){
        if(pt.x>6&&pt.x<12&&pt.y>-14&&pt.y<-8&&pt.z>-1.0&&pt.z<2){
            temp.push_back(pt);
        }
    }
    Eigen::Affine3f transform00;
    transform00 = Eigen::Affine3f::Identity();
    transform00.translation() << 0.0, 0.0, 0.0;
    transform00.rotate(Eigen::AngleAxisf(1.57, Eigen::Vector3f::UnitZ()));
    transform00.rotate(Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()));
    transform00.rotate(Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()));
    pcl::transformPointCloud(temp, temp, transform00);

    if(frames3.size()==0)
        return;
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
//    icp.setMaxCorrespondenceDistance (0.2);
// Set the maximum number of iterations (criterion 1)
    icp.setMaximumIterations (5000);
// Set the transformation epsilon (criterion 2)
    icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
//    icp.setEuclideanFitnessEpsilon (0.1);

    icp.setInputCloud(temp.makeShared());
    mtx.lock();
    icp.setInputTarget(frames3.makeShared());
    mtx.unlock();
    pcl::PointCloud<pcl::PointXYZI> final;
    icp.align(final);
    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
              icp.getFitnessScore() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;
    pubpc("source",temp);
    pubpc("calibed",final);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "bag2pcd");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/pointcloud_lidar3", 10, velodyne_callback3);
    ros::Subscriber sub2 = nh.subscribe("/pointcloud_lidar2", 10, velodyne_callback);

    //注意是否转坐标
    transform0 = Eigen::Affine3f::Identity();
//    2.6, 0.0, 0.0, 0.0, 0.01, 0.0
// 0.88252,0.85335,0.0556839,-0.0139905,-0.0157,1.57237
// 0.684717,-0.270158,0.00258499,-3.13517,-3.14022,1.55372
     transform0.translation() << 2.6, 0.0, 0.0;
//     transform0.rotate(Eigen::AngleAxisf(1.55372, Eigen::Vector3f::UnitZ()));
//     transform0.rotate(Eigen::AngleAxisf(-3.14022, Eigen::Vector3f::UnitY()));
//     transform0.rotate(Eigen::AngleAxisf(-3.13517, Eigen::Vector3f::UnitX()));

    ros::spin();

    return 0;
}
